﻿/*=========================================================================
 * AUTH: Matthew Baum, 9753095
 * FILE: LevelGraph.cs
 * DATE: February-18-2012
 * LAST: April-07-2012
 * DESC: Defines a path-finding graph, and nodes.
 *=========================================================================*/

using System;
using System.Collections.Generic;
using Microsoft.Xna.Framework;
using Microsoft.Xna.Framework.Content;
using Microsoft.Xna.Framework.Graphics;

namespace RisingThreat
{
    // Class defining a path-finding graph Node
    class LevelGraphNode
    {
        // Static ID counter
        public static int ID_COUNTER = 0;

        // ID/name of node
        private int ID;
        
        // Position
        public Vector3 position;

        // Edges to other nodes
        private List<LevelGraphEdge> edges = new List<LevelGraphEdge>(8);

        // FOR A*: cost-so-far
        public float costSoFar;

        // FOR A*: estimated-total-cost
        public float estimatedTotalCost;

        // FOR A*: source edge
        private LevelGraphEdge sourceEdge;

        // Flag: is start node for A*
        public bool isStart = false;

        // Flag: has been inspected by A*
        public bool isTouched = false;

        // Flag: node is goal node for A*
        public bool isGoal = false;

        // CHAR FLAG (underlying tile type) used in edge generation
        private char tile_flag;

        // CONSTRUCTOR(S)
        public LevelGraphNode(float xPos, float zPos, char tile)
        {
            // Set tile-type flag
            tile_flag = tile;

            // Set node position
            position = new Vector3(xPos, 0, zPos);

            // Set ID
            ID = ID_COUNTER;
            ++ID_COUNTER;
        }

        // GET tile flag
        public char getTileFlag()
        {
            return tile_flag;
        }

        // Get distance to another node
        public float getDistanceTo(LevelGraphNode other)
        {
            // Float to hold distance
            float dist = (position - other.position).Length();

            // Return the distance
            return dist;
        }

        // Get distance to a Vector3
        public float getDistanceTo(Vector3 location)
        {
            // Float to hold distance
            float dist = (position - location).Length();

            // Return the distance
            return dist;
        }

        // Function used to add connectivity to neighbouring nodes
        public void addEdge(LevelGraphEdge edge)
        {
            edges.Add(edge);
        }

        // Get neighbour, given an edge
        public LevelGraphNode getNeighbour(LevelGraphEdge edge)
        {
            // Check first node on edge
            if (! edge.getFromNode().Equals(this))
            {
                // Return first node on edge
                return edge.getFromNode();
            }

            // Return second node on edge
            return edge.getToNode();
        }

        // Clear special status flags
        public void clearFlags()
        {
            // Clear start status
            isStart = false;

            // Clear goal status
            isGoal = false;

            // Clear touched status
            isTouched = false;
        }

        // GETTERS AND SETTERS
        // Get position
        public Vector3 getPosition()
        {
            return position;
        }

        // Set goal node
        public void setGoal(bool set)
        {
            isGoal = set;
        }

        // Get goal value
        public bool getGoal()
        {
            return isGoal;
        }

        // Get ID
        public int getID()
        {
            return ID;
        }

        // FOR A*: SET cost-so-far
        public void setCostSoFar(float val)
        {
            costSoFar = val;
        }

        // FOR A*: GET cost-so-far
        public float getCostSoFar()
        {
            return costSoFar;
        }

        // FOR A*: SET e.t.c.
        public void setEstimatedTotalCost(LevelGraphNode goal)
        {
            estimatedTotalCost = costSoFar + getDistanceTo(goal);
        }

        // FOR A*: GET e.t.c.
        public float getEstimatedTotalCost()
        {
            return estimatedTotalCost;
        }

        // FOR A*: SET source edge
        public void setSourceEdge(LevelGraphEdge edge)
        {
            sourceEdge = edge;
        }

        // FOR A*: GET source edge
        public LevelGraphEdge getSourceEdge()
        {
            return sourceEdge;
        }

        // FOR A*: GET edges
        public List<LevelGraphEdge> getEdges()
        {
            return edges;
        }

        // Print
        public override string ToString()
        {
            return "NODE: " + ID;
        }
    }

    // Class defining an edge in a path-finding graph
    class LevelGraphEdge
    {
        // FROM Node
        public LevelGraphNode fromNode;

        // TO Node
        public LevelGraphNode toNode;

        // Weight
        private float weight;

        // Vertex buffer used to draw the edge
        VertexBuffer vertexBuffer;

        // CONSTRUCTOR(S)
        public LevelGraphEdge(LevelGraphNode from, LevelGraphNode to, GraphicsDeviceManager graphics)
        {
            // Set end nodes
            fromNode = from;
            toNode = to;

            // Calculate the weight, based on distance
            calculateDistanceWeight();

            // Set the vertex buffer
            setVertexBuffer(graphics);
        }

        // Calculate weight based on distance
        private void calculateDistanceWeight()
        {
            Vector3 vectorDist = fromNode.getPosition() - toNode.getPosition();

            // Set the weight
            weight = vectorDist.Length();
        }

        // Set the vertex buffer to draw this edge
        private void setVertexBuffer(GraphicsDeviceManager graphics)
        {
            VertexPositionColor[] vertices = new VertexPositionColor[2];
            vertices[0] = new VertexPositionColor(fromNode.getPosition() + new Vector3(0,0.2f,0), Color.Navy);
            vertices[1] = new VertexPositionColor(toNode.getPosition() + new Vector3(0,0.2f,0), Color.Navy);
 
            vertexBuffer = new VertexBuffer(graphics.GraphicsDevice, typeof(VertexPositionColor), 2, BufferUsage.WriteOnly);
            vertexBuffer.SetData<VertexPositionColor>(vertices);
        }

        // Draw the edge
        public void draw(GraphicsDeviceManager graphics, Effect effect)
        {
            graphics.GraphicsDevice.SetVertexBuffer(vertexBuffer);
 
            RasterizerState rasterizerState = new RasterizerState();
            rasterizerState.CullMode = CullMode.None;
            graphics.GraphicsDevice.RasterizerState = rasterizerState;
 
            foreach (EffectPass pass in effect.CurrentTechnique.Passes)
            {
                pass.Apply();
                graphics.GraphicsDevice.DrawPrimitives(PrimitiveType.LineList, 0, 1);
            }
        }

        // GETTERS
        public LevelGraphNode getFromNode()
        {
            return fromNode;
        }

        public LevelGraphNode getToNode()
        {
            return toNode;
        }

        public float getWeight()
        {
            return weight;
        }

        // Print
        public override string ToString()
        {
            return fromNode.getID() + " <----> " + toNode.getID();
        }
    }

    // Class defining a path-finding graph
    class LevelGraph
    {
        // STATIC array used to generate graph edges
        private static LevelGraphNode[,] node_grid;

        // List of Nodes
        public Dictionary<int, LevelGraphNode> nodes;

        // Models used as markers to display nodes
        private Model marker;
        private Model start_marker;
        private Model touched_marker;
        private Model goal_marker;

        // Reference to the Graphics device manager
        GraphicsDeviceManager graphics;

        // CONSTRUCTOR(S)
        public LevelGraph(ContentManager Content, GraphicsDeviceManager gdm, int width, int depth)
        {
            // BUG fix
            LevelGraphNode.ID_COUNTER = 0;

            // Cache gdm
            graphics = gdm;

            // INIT the node grid for generating edges (do NOT do this often!)
            node_grid = new LevelGraphNode[depth, width];

            // Set graph node initial capacity
            nodes = new Dictionary<int, LevelGraphNode>(width * depth);

            // Load content
            loadContent(Content);
        }

        // Automatically generate graph edges!
        public void generateEdges()
        {
            // FOR each ROW (traverse level depth)
            for(int i = 0; i != node_grid.GetUpperBound(0); ++i)
            {
                // FOR each COLUMN (traverser level width)
                for(int j = 0; j != node_grid.GetUpperBound(1); ++j)
                {
                    // Generate edges for the current node, IF NOT NULL
                    if(node_grid[i, j] != null)
                    {
                        generateEdgesForNode(i, j);
                    }
                }
            }
        }

        /*--------------------------------------------------
         * PRIVATE HELPER to generate edges for SOME NODE,
         * based on underlying tile type
         * 
         * - directions are:
         *   - 0 : south-west
         *   - 1 : west
         *   - 2 : north-west
         *   - 3 : north
         *--------------------------------------------------*/ 
        private void generateEdgesForNode(int zIndex, int xIndex)
        {
            // Get the current node
            LevelGraphNode sourceNode = node_grid[zIndex, xIndex];

            // SWITCH based on input value
            switch(sourceNode.getTileFlag())
            {
                // BARE FLOOR TILE
                case '0':
                // START tile
                case 'S':
                // GOAL tile
                case 'G':
                // BOTTOM WALL
                case '3':
                // RIGHT WALL
                case '4':
                // CORNER 3 (bottom and Right walls)
                case '7':

                    // Send out connections in each direction
                    connectNodes(sourceNode, node_grid[zIndex + 1, xIndex - 1], 0);
                    connectNodes(sourceNode, node_grid[zIndex, xIndex - 1],     1);
                    connectNodes(sourceNode, node_grid[zIndex - 1, xIndex - 1], 2);
                    connectNodes(sourceNode, node_grid[zIndex - 1, xIndex],     3);

                break;

                // TOP WALL
                case '1':
                // CORNER 4 (Right and top walls)
                case '8':

                    // Send out connections: 0,1
                    connectNodes(sourceNode, node_grid[zIndex + 1, xIndex - 1], 0);
                    connectNodes(sourceNode, node_grid[zIndex, xIndex - 1], 1);

                break;

                // PIPE 2 (top and bottom walls)
                case 'A':
                // END 4 (bottom-right-top walls)
                case 'E':

                    // Send out connections: 1
                    connectNodes(sourceNode, node_grid[zIndex, xIndex - 1],     1);

                    break;

                // LEFT WALL
                case '2':
                // PIPE 1 (Left and right walls)
                case '9':
                // CORNER 2 (left-bottom walls)
                case '6':
                // END 3 (left-bottom-right walls)
                case 'D':

                    // Send out connections: 3
                    connectNodes(sourceNode, node_grid[zIndex - 1, xIndex],     3);

                    break;
            }
        }

        /*--------------------------------------------------
         * Private helper to connect to nodes by an edge
         * 
         * - direction is:
         *   - 0 : south-west
         *   - 1 : west
         *   - 2 : north-west
         *   - 3 : north
         *--------------------------------------------------*/ 
        private void connectNodes(LevelGraphNode source, LevelGraphNode target, byte direction)
        {
            // IF target is NULL, return
            if(target == null)
            {
                return;
            }

            // SWITCH on target tile flag
            switch(target.getTileFlag())
            {
                // BARE FLOOR TILE(S)
                case '0':
                case 'S':
                case 'G':
                // TOP WALL
                case '1':
                // LEFT WALL
                case '2':
                // CORNER 1 (top-left walls)
                case '5':

                    // Accept ANY incoming connection
                    addEdge(source.getID(), target.getID() );

                break;

                // BOTTOM WALL
                case '3':
                // CORNER 2 (left-bottom walls)
                case '6':

                    // Accept: 0,1
                    if (direction == 0 || direction == 1)
                    {
                        addEdge(source.getID(), target.getID());
                    }

                break;

                // PIPE 2 (top-bottom walls)
                case 'A':
                // END 2 (top-left-bottom walls)
                case 'C':

                    // Accept: 1
                    if(direction == 1)
                    {
                        addEdge(source.getID(), target.getID() );
                    }

                    break;

                // RIGHT WALL
                case '4':
                // PIPE 1 (left-right walls)
                case '9':

                    // Accept: 3
                    if(direction == 3)
                    {
                        addEdge(source.getID(), target.getID() );
                    }

                    break;

                // CORNER 4 (right-top walls)
                case '8':
                // END 1 (top-left-right walls)
                case 'B':

                    // Accept: 3
                    if(direction == 3)
                    {
                        addEdge(source.getID(), target.getID() );
                    }

                    break;
            }

        }

        public void clearNodes()
        {
            for (int i = 0; i < nodes.Count; i++)
            {
                nodes[i].costSoFar = 0;
                nodes[i].estimatedTotalCost = 0;
            }
        }


        // Find the node nearest to a given random node
        public LevelGraphNode findNearestNode(LevelGraphNode randomNode)
        {
            // Get collection of nodes
            Dictionary<int, LevelGraphNode>.ValueCollection nodeCollection = nodes.Values;

            // Dummy start value for the shortest distance found
            float shortestDist = float.MaxValue;

            // Temp var to hold distance calculations
            float tempDist;

            // Variable to store best match
            LevelGraphNode bestNode = null;

            // Iterate over all nodes, and find the one nearest to the random node
            foreach(LevelGraphNode node in nodeCollection)
            {
                // Calculate distance
                tempDist = randomNode.getDistanceTo(node);

                if(tempDist < shortestDist)
                {
                    // Set new shortest distance
                    shortestDist = tempDist;

                    // Set new best node
                    bestNode = node;
                }
            }

            // Return the best node found
            return bestNode;
        }

        // Find the node nearest to a given location
        public LevelGraphNode findNearestNode(Vector3 location)
        {
            // Get collection of nodes
            Dictionary<int, LevelGraphNode>.ValueCollection nodeCollection = nodes.Values;

            // Dummy start value for the shortest distance found
            float shortestDist = float.MaxValue;

            // Temp var to hold distance calculations
            float tempDist;

            // Variable to store best match
            LevelGraphNode bestNode = null;

            // Iterate over all nodes, and find the one nearest to the random node
            foreach (LevelGraphNode node in nodeCollection)
            {
                // Calculate distance
                tempDist = node.getDistanceTo(location);

                if (tempDist < shortestDist)
                {
                    // Set new shortest distance
                    shortestDist = tempDist;

                    // Set new best node
                    bestNode = node;
                }
            }

            // Return the best node found
            return bestNode;
        }

        // A* ALGORITHM
        public Stack<LevelGraphEdge> aStar(LevelGraphNode start, LevelGraphNode goal)
        {
            // Set start node to a special color
            start.isStart = true;

            // Set goal node to a special color
            goal.isGoal = true;

            // Current node (set to start node)
            LevelGraphNode current = start;

            // Open list (dictionary)
            Dictionary<int, LevelGraphNode> openList = new Dictionary<int,LevelGraphNode>(nodes.Count);

            // Closed list (dictionary)
            Dictionary<int, LevelGraphNode> closedList = new Dictionary<int,LevelGraphNode>(nodes.Count);

            // List to hold node connections
            List<LevelGraphEdge> connections;

            // Node to hold neighbour nodes
            LevelGraphNode neighbour;

            // Float to hold nieghbour-node cost
            float neighbourCost;

            // Set cost so far of the start node to zero
            start.setCostSoFar(0);
            start.setEstimatedTotalCost(goal);

            // Add start node to the open list
            openList.Add(start.getID(), start);

            // Iterate through the open list, until empty
            while(openList.Count > 0)
            {
                // Get node with smallest estimated total cost from the open list
                current = findLowestCostNode(openList);

                // TERMINATION CONDITION: We have reached the goal!
                if(current.Equals(goal))
                {
                    break;
                }

                // Get connections from the current node
                connections = current.getEdges();

                // Loop through the edges
                foreach(LevelGraphEdge connection in connections)
                {
                    // Get neighbour node
                    neighbour = current.getNeighbour(connection);

                    // Calculate cost to neighbour node
                    neighbourCost = current.getCostSoFar() + connection.getWeight();

                    // IF neighbour is in the closed list
                    if(closedList.ContainsKey(neighbour.getID() ) )
                    {
                        // SKIP node: IF current cost is NOT BETTER
                        if(neighbour.getCostSoFar() <= neighbourCost)
                        {
                            continue;
                        }

                        // ELSE
                        else
                        {
                            // Remove from closed list
                            closedList.Remove(neighbour.getID() );
                        }
                    }

                    // ELSE: inspect the open list
                    else if(openList.ContainsKey(neighbour.getID() ) )
                    {
                        // Get neighbour node
                        neighbour = current.getNeighbour(connection);

                        // Calculate cost to neighbour node
                        neighbourCost = current.getCostSoFar() + connection.getWeight();

                        // SKIP node: IF current cost is NOT BETTER
                        if(neighbour.getCostSoFar() <= neighbourCost)
                        {
                            continue;
                        }

                        neighbour.setCostSoFar(neighbourCost);
                        neighbour.setSourceEdge(connection);
                        neighbour.setEstimatedTotalCost(goal);
                    }

                    // ELSE: Neighbour node is UNVISITED!
                    else
                    {
                        // This is our first visit, so update node fields for A*
                        neighbour.setCostSoFar(neighbourCost);
                        neighbour.setSourceEdge(connection);
                        neighbour.setEstimatedTotalCost(goal);

                        // Node is inspected: Add neighbour to the open list
                        openList.Add(neighbour.getID(), neighbour);

                        // Color the inspected node
                        neighbour.isTouched = true;
                    }
                }

                // We have inspected all of the neighbouring nodes (current node is processed)
                // Remove from open list
                openList.Remove(current.getID() );

                // Add to closed list
                closedList.Add(current.getID(), current );
            }

            // We have reached an EXIT CONDITION
            // IF we have NOT reached the goal, the graph is unsolvable!
            if(current != goal)
            {
                return null;
            }

            // ELSE: we reached the goal!
            // Reconstruct the activePath traversed
            else
            {
                return reconstructPath(current, start);
            }
        }

        // HELPER for A*: Find node on the open list with the lowest estimated total cost
        private LevelGraphNode findLowestCostNode(Dictionary<int, LevelGraphNode> openList)
        {
            // Lowest cost node
            LevelGraphNode lowestCostNode = null;

            // Lowest cost
            float lowestCost = float.MaxValue;

            // Get values from the open list
            Dictionary<int, LevelGraphNode>.ValueCollection openNodes = openList.Values;

            // Find the lowest-cost node
            foreach(LevelGraphNode node in openNodes)
            {
                if(node.getEstimatedTotalCost() < lowestCost)
                {
                    lowestCostNode = node;
                    lowestCost = node.getEstimatedTotalCost();
                }
            }

            // Return the lowest-cost node
            return lowestCostNode;
        }

        // Helper for A*: Reconstruct the activePath
        private Stack<LevelGraphEdge> reconstructPath(LevelGraphNode current, LevelGraphNode start)
        {
            // Stack to hold the traversal activePath
            Stack<LevelGraphEdge> path = new Stack<LevelGraphEdge>();

            // Find all edges used (trace back to start node)
            while(current != start)
            {
                // Push the current source edge
                path.Push(current.getSourceEdge() );

                // Move back along the activePath
                current = current.getNeighbour(current.getSourceEdge() );
            }

            // Return the stack holding the traversal activePath
            return path;
        }

        // Add a node to the graph
        public void addNode(LevelGraphNode node, int zIndex, int xIndex)
        {
            // Add ONLY IF ID is UNIQUE
            if(! nodes.ContainsKey(node.getID() ) )
            {
                // Add to the node dictionary
                nodes.Add(node.getID(), node);

                // Add to the 2D array for edge generation
                node_grid[zIndex, xIndex] = node;
            }
        }

        // Add an edge
        public bool addEdge(int fromID, int toID)
        {
            LevelGraphNode fromNode;
            LevelGraphNode toNode;
            LevelGraphEdge edge;

            // Boolean success result
            bool success = false;

            // Add edge, if both nodes exist
            if
            (
                nodes.TryGetValue(fromID, out fromNode) && 
                nodes.TryGetValue(toID, out toNode)
            )
            {
                // Create a new edge
                edge = new LevelGraphEdge(fromNode, toNode, graphics);

                // Add edge to each node concerned
                fromNode.addEdge(edge);
                toNode.addEdge(edge);

                // Success
                success = true;
            }

            // Return success result
            return success;
        }

        // Clear all special marker status on nodes
        public void resetMarkers()
        {
            // Get collection of nodes
            Dictionary<int, LevelGraphNode>.ValueCollection nodeCollection = nodes.Values;

            // Iterate over all nodes, clear special status
            foreach(LevelGraphNode node in nodeCollection)
            {
                node.clearFlags();
            }
        }

        // Load graph content
        private void loadContent(ContentManager Content)
        {
            // Load node merker model
            marker = Content.Load<Model>("marker");
            start_marker = Content.Load<Model>("start_marker");
            touched_marker = Content.Load<Model>("touched_marker");
            goal_marker = Content.Load<Model>("goal_marker");
        }

        // Draw the nodes in the graph
        public void draw(Matrix viewMatrix, Matrix projectionMatrix, Effect effect)
        {
            // Iterate accross all nodes
            Dictionary<int, LevelGraphNode>.ValueCollection nodeCollection = nodes.Values;

            // Edges for each node
            List<LevelGraphEdge> nodeEdges;

            foreach (LevelGraphNode node in nodeCollection)
            {
                if(node.isStart)
                {
                    // Draw a marker at that position
                    drawMarker(start_marker, node.getPosition(), viewMatrix, projectionMatrix);
                }
                else if(node.isGoal)
                {
                    // Draw a marker at that position
                    drawMarker(goal_marker, node.getPosition(), viewMatrix, projectionMatrix);
                }
                else if(node.isTouched)
                {
                    // Draw a marker at that position
                    drawMarker(touched_marker, node.getPosition(), viewMatrix, projectionMatrix);
                }
                else
                {
                    // Draw a marker at that position
                    drawMarker(marker, node.getPosition(), viewMatrix, projectionMatrix);
                }

                nodeEdges = node.getEdges();

                foreach(LevelGraphEdge edge in nodeEdges)
                {
                    edge.draw(graphics, effect);
                }
            }
        }

        // Private helper to draw a node marker
        private void drawMarker(Model marker, Vector3 pos, Matrix viewMatrix, Matrix projectionMatrix)
        {
            // Copy any parent transforms.
            Matrix[] transforms = new Matrix[marker.Bones.Count];
            marker.CopyAbsoluteBoneTransformsTo(transforms);

            // Draw the model1. A model can have multiple meshes, so loop.
            foreach (ModelMesh mesh in marker.Meshes)
            {
                // This is where the mesh orientation is set, as well 
                // as our activeCamera and projection.
                foreach (BasicEffect effect in mesh.Effects)
                {
                    effect.EnableDefaultLighting();

                    effect.World = transforms[mesh.ParentBone.Index]
                        * Matrix.CreateScale(0.5f)
                        * Matrix.CreateTranslation(pos);

                    effect.View = viewMatrix;

                    effect.Projection = projectionMatrix;
                }

                // Draw the mesh, using the effects set above.
                mesh.Draw();
            }
        }

        // Get a node
        public void getNode(int ID, out LevelGraphNode node)
        {
            nodes.TryGetValue(ID, out node);
        }
    }
}
